#include "cmsis_os.h"
#include "pid.h"
#include "arm_math.h"
#include "CAN_receive.h"
#include "INS_task.h"
#include "main.h"
#include "remote_control.h"
#include "nucCommu.h"

#include "naive_gimbal_task.h"
#include "robot_total_mode.h"
#include "RobotStructure.h"
#include "usb_task.h"
#include "OLED.h"
#include "user_lib.h"
#include "detect_task.h"

#define GIMBAL_TASK_INIT_TIME 2000
#define GIMBAL_TASK_CTRL_TIME 2

// 通过ST studio调试PID参数
// #define FINDING_PID

// #define ZERO_CURRENT_SAFE

#define SAFE_AUTO_SHIFT_TIME 1500   //自动模式时，当装甲板丢失时，使用手动控制，
                //但不清空操作手鼠标偏移量。当重新从手动进入自动模式模式时，操作手可能因为来不及反应
                //导致偏移量破坏。因此设计安全时间，当重新进入时，有一段时间无视操作手新增偏移量。
                //这段时间为SAFE_AUTO_SHIFT_TIME 单位ms


//****************************机器人测试获得值*************//
#if defined OLD_INFANTRY

    #define PITCH_TEST_RAD_MIN (-0.30f)
    #define PITCH_TEST_RAD_MAX  (0.46f)


    // 输入角速度 rad/s 、输出电压 int16_t 的PID系数
    #define PITCH_SPD_KP 14000.0f
    #define PITCH_SPD_KI 0.0f
    #define PITCH_SPD_KD 0.0f

    #define PITCH_VOLT_MAX_OUT  30000.0f
    #define PITCH_VOLT_MAX_IOUT 5000.0f

    #define PITCH_AGL_KP 5.0f
    #define PITCH_AGL_KI 0.0f
    #define PITCH_AGL_KD 0.0f

    #define PITCH_AGL_SPD_MAX_OUT (50.0f)
    #define PITCH_AGL_SPD_MAX_IOUT (0.7f)


    #define YAW_SPD_KP 15000.0f

    #define YAW_SPD_KI 0000.0f
    #define YAW_SPD_KD 000.0f

    #define YAW_VOLT_MAX_OUT  30000.0f
    #define YAW_VOLT_MAX_IOUT 5000.0f

    //输入角度 rad ，输出角速度rad/s 的PID系数
    #define YAW_AGL_KP 10.00f
    #define YAW_AGL_KI 0.0f
    #define YAW_AGL_KD 0.0f

    #define YAW_AGL_SPD_MAX_OUT (50.0f)
    #define YAW_AGL_SPD_MAX_IOUT (0.7f)



    //一阶滤波器的参数,专门给。这两个值越大，理论上越丝滑
    #define PITCH_SPD_FILTER_NUM 15.0f
    #define YAW_SPD_FILTER_NUM 15.0f

#elif defined NEW_INFANTRY

    #define PITCH_TEST_RAD_MIN (-0.15f)
    #define PITCH_TEST_RAD_MAX  (0.48f)

    // 输入角速度 rad/s 、输出电压 int16_t 的PID系数
    #define PITCH_SPD_KP 15000.0f
    #define PITCH_SPD_KI 0.0f
    #define PITCH_SPD_KD 0.0f

    #define PITCH_VOLT_MAX_OUT  30000.0f
    #define PITCH_VOLT_MAX_IOUT 5000.0f

    #define PITCH_AGL_KP 9.0f
    #define PITCH_AGL_KI 0.0f
    #define PITCH_AGL_KD 0.0f

    #define PITCH_AGL_SPD_MAX_OUT (40.0f)
    #define PITCH_AGL_SPD_MAX_IOUT (1.0f)

    #define YAW_SPD_KP 24000.0f
    #define YAW_SPD_KI 0.0f
    #define YAW_SPD_KD 0.0f

    #define YAW_VOLT_MAX_OUT  30000.0f
    #define YAW_VOLT_MAX_IOUT 5000.0f


    //输入角度 rad ，输出角速度rad/s 的PID系数
    #define YAW_AGL_KP 14.0f
    #define YAW_AGL_KI 0.0f
    #define YAW_AGL_KD 0.0f
  

    #define YAW_AGL_SPD_MAX_OUT (40.0f)
    #define YAW_AGL_SPD_MAX_IOUT (5.0f)

    //一阶滤波器的参数,专门给。这两个值越大，理论上越丝滑
    #define PITCH_SPD_FILTER_NUM 15.0f
    #define YAW_SPD_FILTER_NUM 15.0f



#elif defined SENTRY

    #define PITCH_TEST_RAD_MIN (-0.19f)
    #define PITCH_TEST_RAD_MAX  (0.45f)


    // 输入角速度 rad/s 、输出电压 int16_t 的PID系数
    #define PITCH_SPD_KP 10000.0f
    #define PITCH_SPD_KI 0.0f
    #define PITCH_SPD_KD 0.0f

    #define PITCH_VOLT_MAX_OUT  30000.0f
    #define PITCH_VOLT_MAX_IOUT 5000.0f


    #define PITCH_AGL_KP 10.00f
    #define PITCH_AGL_KI 0.0f
    #define PITCH_AGL_KD 0.0f

    #define PITCH_AGL_SPD_MAX_OUT (30.0f)
    #define PITCH_AGL_SPD_MAX_IOUT (0.7f)

    #define YAW_SPD_KP 18000.0f
    #define YAW_SPD_KI 0000.0f
    #define YAW_SPD_KD 000.0f

    #define YAW_VOLT_MAX_OUT  30000.0f
    #define YAW_VOLT_MAX_IOUT 5000.0f


    //输入角度 rad ，输出角速度rad/s 的PID系数
    #define YAW_AGL_KP 30.0f
    #define YAW_AGL_KI 0.0f
    #define YAW_AGL_KD 0.0f

    #define YAW_AGL_SPD_MAX_OUT (50.0f)
    #define YAW_AGL_SPD_MAX_IOUT (0.7f)


    //一阶滤波器的参数,专门给。这两个值越大，理论上越丝滑
    #define PITCH_SPD_FILTER_NUM 15.0f
    #define YAW_SPD_FILTER_NUM 15.0f

#else
    // error
#endif

//********************************************************//


//*****************************遥控器通道选项**************//
#define GIMBAL_YAW_OR_CHASSIS_W_CHANNEL 0     //右边的左右摇杆
#define GIMBAL_PITCH_CHANNEL 1  //右边的前后摇杆
#define RC_DEADLINE 10  //低于这个值的摇杆读数一律为0
#define TO_CHASSIS_COEF 1

#define rc_deadband_limit(input, output, dealine)        \
    {                                                    \
        if ((input) > (dealine) || (input) < -(dealine)) \
        {                                                \
            (output) = (input);                          \
        }                                                \
        else                                             \
        {                                                \
            (output) = 0;                                \
        }                                                \
    }


//****************************控制量计算系数**************//
// 使用遥控器和键盘控制云台，被控制量为速度
// 遥控器和键盘控制都是速度，控制量和速度
// NUC控制时，目标控制量为角度
#define YAW_RC_SEN      -0.000004f
#define PITCH_RC_SEN    -0.000005f //0.005
#define YAW_MOUSE_SEN   -0.00005f
#define PITCH_MOUSE_SEN -0.00005f

#define virtual_YAW_RC  660
//估计数量级

// 遥控器最大值660
// 0.5秒(500ms)角度增量 1.7rad
//5.1515151515151515151515151515152e-6

//鼠标最大值：32767
//0.5秒(500ms)角度增量 1.7rad
// 1.0376293221839045381023590807825e-7

//限幅函数 //test done
#define range_limit_inside(toBeLimited, range)                      \
    {                                                               \
         if((toBeLimited)>(range))                                  \
        {                                                           \
            (toBeLimited)=(range);                                  \
        }                                                           \
        else if((toBeLimited)<-(range))                             \
        {                                                           \
            (toBeLimited)=-(range);                                 \
        }                                                           \
    }



struct gimbalMotorCtrl_s{
    //rads
    const fp32 * anglePoint;          //从姿态解算任务得到的角度位置
    const fp32 * gyroPointer;          //陀螺仪角速度指针
    fp32 nowAbsoluteAngle;      //当前姿态角
    fp32 radSpeed;              //角速度 rad/s
    fp32 wantedAbsoluteAngle;   //目标姿态角

    //PIDs
    pid_type_def spd_pid;       //用于控制速度的PID环。输入角速度 rad/s, 输出GM6020电压 int16
    pid_type_def agl_pid;       //用于控制角度的PID环。输入角度 rad,输出角速度rad/s

    //控制量
    int16_t giveVolt;       //GM6020发送电压
    uint8_t zeroVoltMark;

    // 一阶滤波器，过滤角速度
    first_order_filter_type_t spd_filter;
};

static struct gimbalMotorCtrl_s gimbalPitchCtrl,gimbalYawCtrl;
static struct gimbalMotorCtrl_s *gimbalCtrl[2]={&gimbalPitchCtrl,&gimbalYawCtrl};

#ifdef FINDING_PID
fp32 yawSpdKp = YAW_SPD_KP, yawSpdKi = YAW_SPD_KI, yawSpdKd = YAW_SPD_KD
    , pitchSpdKp = PITCH_SPD_KP, pitchSpdKi = PITCH_SPD_KI, pitchSpdKd = PITCH_SPD_KD;
    
fp32 yawAglKp = YAW_AGL_KP, yawAglKi = YAW_AGL_KI, yawAglKd = YAW_AGL_KD
    , pitchAglKp = PITCH_AGL_KP, pitchAglKi = PITCH_AGL_KI, pitchAglKd = PITCH_AGL_KD;
#endif

fp32 yawSpdPIDco[3]={YAW_SPD_KP,YAW_SPD_KI,YAW_SPD_KD};
fp32 pitchSpdPIDco[3]={PITCH_SPD_KP,PITCH_SPD_KI,PITCH_SPD_KD};
fp32 yawAglPIDco[3]={YAW_AGL_KP,YAW_AGL_KI,YAW_AGL_KD};
fp32 pitchAglPIDco[3]={PITCH_AGL_KP,PITCH_AGL_KI,PITCH_AGL_KD};

static const enum RobotState_e *robotMode;
//控制指针
static const RC_ctrl_t *rc_p;   //遥控器位置指针，需要初始化
static const toSTM32_t *nuc_p;  //NUC数据位置。未来制造全自动机器人时需要

// 抄一个过来
// https://blog.csdn.net/CSDN_X_W/article/details/90289021

//1. 结构体类型定义
typedef struct 
{
    float LastP;//上次估算协方差 初始化值为0.02
    float Now_P;//当前估算协方差 初始化值为0
    float out;//卡尔曼滤波器输出 初始化值为0
    float Kg;//卡尔曼增益 初始化值为0
    float Q;//过程噪声协方差 初始化值为0.00001
    float R;//观测噪声协方差 初始化值为0.00003
}KFP;//Kalman Filter parameter

//2. 以高度为例 定义卡尔曼结构体并初始化参数
KFP KFP_height={0.02,0,0,0.1,0.0001,0.0003};

KFP KFP_pitch_agl={0.02,0,0,0.05,0.0001,0.0003};

KFP KFP_pitch_wantedagl={0.02,0,0,0.9,0.0001,0.0003};
KFP KFP_yaw_wantedagl={0.02,0,0,0.9,0.0001,0.0003};

/**
 *卡尔曼滤波器
 *@param KFP *kfp 卡尔曼结构体参数
 *   float input 需要滤波的参数的测量值（即传感器的采集值）
 *@return 滤波后的参数（最优值）
 */
 float kalmanFilter(KFP *kfp,float input)
 {
     //预测协方差方程：k时刻系统估算协方差 = k-1时刻的系统协方差 + 过程噪声协方差
     kfp->Now_P = kfp->LastP + kfp->Q;
     //卡尔曼增益方程：卡尔曼增益 = k时刻系统估算协方差 / （k时刻系统估算协方差 + 观测噪声协方差）
    //  kfp->Kg = kfp->Now_P / (kfp->Now_P + kfp->R);

     //更新最优值方程：k时刻状态变量的最优值 = 状态变量的预测值 + 卡尔曼增益 * （测量值 - 状态变量的预测值）
     kfp->out = kfp->out + kfp->Kg * (input -kfp->out);//因为这一次的预测值就是上一次的输出值
     //更新协方差方程: 本次的系统协方差付给 kfp->LastP 威下一次运算准备。
     kfp->LastP = (1-kfp->Kg) * kfp->Now_P;
     return kfp->out;
 }

/**
 *调用卡尔曼滤波器 实践
 */
// int height;
// int kalman_height=0;
// kalman_height = kalmanFilter(&KFP_height,(float)height);,

int NUCFoundArmor(void)
{
    return(nuc_p->found_armor == 1);
}

static void initGimbalCtrls(void)
{
    uint8_t i;
    //初始化绝对角度位置
#if defined OLD_INFANTRY
    gimbalYawCtrl.anglePoint=get_INS_angle_point();         //第一个角度
    gimbalPitchCtrl.anglePoint=(&(get_INS_angle_point()[2])); //第三个角度
    gimbalYawCtrl.gyroPointer=&(get_gyro_data_point()[2]);
    gimbalPitchCtrl.gyroPointer=&(get_gyro_data_point()[0]);
#elif defined NEW_INFANTRY
    gimbalYawCtrl.anglePoint=get_INS_angle_point();         //第一个角度
    gimbalPitchCtrl.anglePoint=(&(get_INS_angle_point()[1])); //第二个角度
    gimbalYawCtrl.gyroPointer=&(get_gyro_data_point()[2]);
    gimbalPitchCtrl.gyroPointer=&(get_gyro_data_point()[1]);
#elif defined SENTRY
    gimbalYawCtrl.anglePoint=get_INS_angle_point();         //第一个角度
    gimbalPitchCtrl.anglePoint=(&(get_INS_angle_point()[2])); //第三个角度
    gimbalYawCtrl.gyroPointer=&(get_gyro_data_point()[2]);
    gimbalPitchCtrl.gyroPointer=get_gyro_data_point();
#else 
    // error
#endif


    // 初始化PID参数
    // 两个双环PID不共用参数
    PID_init(&(gimbalPitchCtrl.spd_pid),PID_POSITION,pitchSpdPIDco,PITCH_VOLT_MAX_OUT,PITCH_VOLT_MAX_IOUT);
    PID_init(&(gimbalYawCtrl.spd_pid),PID_POSITION,yawSpdPIDco,YAW_VOLT_MAX_OUT,YAW_VOLT_MAX_IOUT);
    
    PID_init(&(gimbalPitchCtrl.agl_pid),PID_POSITION,pitchAglPIDco,PITCH_AGL_SPD_MAX_OUT,PITCH_AGL_SPD_MAX_IOUT);
    PID_init(&(gimbalYawCtrl.agl_pid),PID_POSITION,yawAglPIDco,YAW_AGL_SPD_MAX_OUT,YAW_AGL_SPD_MAX_IOUT);

    //滤波初始化
    const static fp32 gimbal_pitch_order_filter[1] = {PITCH_SPD_FILTER_NUM};
    const static fp32 gimbal_yaw_order_filter[1] = {YAW_SPD_FILTER_NUM};

    first_order_filter_init(&(gimbalPitchCtrl.spd_filter), GIMBAL_TASK_CTRL_TIME, gimbal_pitch_order_filter);
    first_order_filter_init(&(gimbalYawCtrl.spd_filter), GIMBAL_TASK_CTRL_TIME, gimbal_yaw_order_filter);

    
    //初始化当前角度和目标角度、pid参数、里程碑栈法数据
    for(i=0;i<2;i++)
    {
        (gimbalCtrl[i])->zeroVoltMark=0;
        (gimbalCtrl[i])->nowAbsoluteAngle=*((gimbalCtrl[i])->anglePoint);
        gimbalCtrl[i]->wantedAbsoluteAngle=(gimbalCtrl[i])->nowAbsoluteAngle;
    }
    PID_clear(&gimbalYawCtrl.agl_pid);
    PID_clear(&gimbalYawCtrl.spd_pid);
    PID_clear(&gimbalPitchCtrl.agl_pid);
    PID_clear(&gimbalPitchCtrl.spd_pid);
    gimbalYawCtrl.radSpeed = 0;
    gimbalPitchCtrl.radSpeed = 0;
    
}

/**
 * @brief 将角度化为(-PI,PI)范围内
 * 
 * @param rawAngle 
 * @return fp32 
 */
fp32 radFormat(fp32 rawAngle)   //test done
{
    while(rawAngle>PI)
        rawAngle-=(2*PI);
    while(rawAngle<(-PI))
        rawAngle+=(2*PI);
    return rawAngle;
}

/**
 * @brief 计算转圈的PID。内部error先循环转化为(-PI,PI)范围
 * 
 * @param pid 
 * @param ref 反馈值
 * @param set 设定值
 * @return fp32 
 */
fp32 gimbal_PID_calc(pid_type_def *pid, fp32 ref, fp32 set)
{
    if (pid == NULL)
    {
        return 0.0f;
    }

    pid->error[2] = pid->error[1];
    pid->error[1] = pid->error[0];
    pid->set = set;
    pid->fdb = ref;
    pid->error[0] = radFormat(set - ref);
    if (pid->mode == PID_POSITION)
    {
        pid->Pout = pid->Kp * pid->error[0];
        pid->Iout += pid->Ki * pid->error[0];
        pid->Dbuf[2] = pid->Dbuf[1];
        pid->Dbuf[1] = pid->Dbuf[0];
        pid->Dbuf[0] = (pid->error[0] - pid->error[1]);
        pid->Dout = pid->Kd * pid->Dbuf[0];
        range_limit_inside(pid->Iout, pid->max_iout);
        pid->out = pid->Pout + pid->Iout + pid->Dout;
        range_limit_inside(pid->out, pid->max_out);
    }
    else if (pid->mode == PID_DELTA)
    {
        pid->Pout = pid->Kp * (pid->error[0] - pid->error[1]);
        pid->Iout = pid->Ki * pid->error[0];
        pid->Dbuf[2] = pid->Dbuf[1];
        pid->Dbuf[1] = pid->Dbuf[0];
        pid->Dbuf[0] = (pid->error[0] - 2.0f * pid->error[1] + pid->error[2]);
        pid->Dout = pid->Kd * pid->Dbuf[0];
        pid->out += pid->Pout + pid->Iout + pid->Dout;
        range_limit_inside(pid->out, pid->max_out);
    }
    return pid->out;
}




void refreshAngleStates(void)
{
    //当前姿态角和角速度
#if defined OLD_INFANTRY
    gimbalYawCtrl.nowAbsoluteAngle  =   *(gimbalYawCtrl.anglePoint);
    gimbalYawCtrl.radSpeed          =   *(gimbalYawCtrl.gyroPointer);
    gimbalPitchCtrl.nowAbsoluteAngle=   -*(gimbalPitchCtrl.anglePoint);
    gimbalPitchCtrl.radSpeed        =   -*(gimbalPitchCtrl.gyroPointer);
#elif defined NEW_INFANTRY
    gimbalYawCtrl.nowAbsoluteAngle  =   *(gimbalYawCtrl.anglePoint);
    gimbalYawCtrl.radSpeed          =   *(gimbalYawCtrl.gyroPointer);
    gimbalPitchCtrl.nowAbsoluteAngle=   kalmanFilter(&KFP_pitch_agl, -*(gimbalPitchCtrl.anglePoint));
    gimbalPitchCtrl.radSpeed        =   kalmanFilter(&KFP_height , -*(gimbalPitchCtrl.gyroPointer));
#elif defined SENTRY
    gimbalYawCtrl.nowAbsoluteAngle  =   *(gimbalYawCtrl.anglePoint);
    gimbalYawCtrl.radSpeed          =   *(gimbalYawCtrl.gyroPointer);
    gimbalPitchCtrl.nowAbsoluteAngle=   *(gimbalPitchCtrl.anglePoint);
    gimbalPitchCtrl.radSpeed        =   *(gimbalPitchCtrl.gyroPointer);
#else 
    // error
#endif
}

/**
 * @brief 根据机器人状态控制pitch和yaw 旋转方向或者传给底盘的速度
 * 
 */
void getControlAngles(void)
{
    static fp32 Manual_selfTargetOffsetPitch=0,Manual_selfTargetOffsetYaw=0;
    static int16_t yaw_channel = 0, pitch_channel = 0;
    // static fp32 Bri_selfTargetOffsetPitch=0,Bri_selfTargetOffsetYaw=0;//待完成：全自动模式的手动矫正程序
    rc_deadband_limit(rc_p->rc.ch[GIMBAL_YAW_OR_CHASSIS_W_CHANNEL], yaw_channel, RC_DEADLINE);
    rc_deadband_limit(rc_p->rc.ch[GIMBAL_PITCH_CHANNEL], pitch_channel, RC_DEADLINE);
    //打包处理鼠标和遥控器对于云台rad速度的控制

    //发现在bad yaw car转到其他模式后，云台会突然转向。这是因为云台没有到程序希望的位置于是，增加一个转换过程。
    // 当上一次模式和这一次模式不同时，将希望的角度设置为当前角度。
    static enum RobotState_e lastMode=RobotState_e_Powerless;   //初始化为无力
    if(lastMode!=(*robotMode))
        gimbalYawCtrl.wantedAbsoluteAngle=gimbalYawCtrl.nowAbsoluteAngle;
    lastMode=*robotMode;
 
//根据模式选择标志
    gimbalYawCtrl.zeroVoltMark=0;
    gimbalPitchCtrl.zeroVoltMark=0;

    if(*robotMode==RobotState_e_Powerless)
    {
        gimbalYawCtrl.zeroVoltMark=1;
        gimbalPitchCtrl.zeroVoltMark=1;
        // Bri_selfTargetOffsetPitch=0;
        // Bri_selfTargetOffsetYaw=0;
        Manual_selfTargetOffsetPitch=0;
        Manual_selfTargetOffsetYaw=0;

    }    
    else if(*robotMode==RobotState_e_Brilliant)
    {
        // Bri_selfTargetOffsetPitch = GIMBAL_TASK_CTRL_TIME*pitch_channel*PITCH_RC_SEN;
        // Bri_selfTargetOffsetYaw = GIMBAL_TASK_CTRL_TIME*yaw_channel*YAW_RC_SEN;
        //  if(NUCFoundArmor())
        // {
            if(gimbal_NUCisAvailable())
            {
                gimbalYawCtrl.wantedAbsoluteAngle = gimbalYawCtrl.nowAbsoluteAngle + nuc_p->yaw; //+ Bri_selfTargetOffsetYaw;
                gimbalPitchCtrl.wantedAbsoluteAngle = gimbalPitchCtrl.nowAbsoluteAngle + nuc_p->pitch; //+ Bri_selfTargetOffsetPitch;
                gimbalClearReadMark();
            }
        // }
        // else
        // {
        //     // gimbalYawCtrl.wantedAbsoluteAngle += YAW_RC_SEN * virtual_YAW_RC;
        //     // gimbalPitchCtrl.wantedAbsoluteAngle = gimbalPitchCtrl.nowAbsoluteAngle;
        // }
    }
    else
    {
        if(!robotIsAuxiliaryTarget())    //当自瞄取消掉的时候，偏移清零
        {
            Manual_selfTargetOffsetPitch=0;
            Manual_selfTargetOffsetYaw=0;
            gimbalYawCtrl.wantedAbsoluteAngle += GIMBAL_TASK_CTRL_TIME*(yaw_channel * YAW_RC_SEN + rc_p->mouse.x * YAW_MOUSE_SEN);
            gimbalPitchCtrl.wantedAbsoluteAngle += GIMBAL_TASK_CTRL_TIME*(pitch_channel * PITCH_RC_SEN + rc_p->mouse.y * PITCH_MOUSE_SEN);
        }
        else// if(robotIsAuxiliaryTarget())//自瞄得到的初始的角度 加上鼠标控制偏移
        {
            static uint32_t lastManualTime=0;
            if((HAL_GetTick()-lastManualTime)>SAFE_AUTO_SHIFT_TIME) //只有过了缓冲时间之后才认定偏移量
            {
                //pitch和yaw偏移,并增加到角度上。
                Manual_selfTargetOffsetPitch+=GIMBAL_TASK_CTRL_TIME*(pitch_channel * PITCH_RC_SEN + rc_p->mouse.y * PITCH_MOUSE_SEN);
                Manual_selfTargetOffsetYaw+=GIMBAL_TASK_CTRL_TIME*(yaw_channel * YAW_RC_SEN + rc_p->mouse.x * YAW_MOUSE_SEN);
            }
            
            //给偏移量限幅
            range_limit_inside(Manual_selfTargetOffsetPitch,PI/3);
            range_limit_inside(Manual_selfTargetOffsetYaw,PI/3);
            if(foundArmor())    //找到了才让云台跟随NUC数据
            {
                if(gimbal_NUCisAvailable())
                {
                    gimbalYawCtrl.wantedAbsoluteAngle=gimbalYawCtrl.nowAbsoluteAngle+ nuc_p->yaw+Manual_selfTargetOffsetYaw;
                    gimbalPitchCtrl.wantedAbsoluteAngle=gimbalPitchCtrl.nowAbsoluteAngle+ nuc_p->pitch+Manual_selfTargetOffsetPitch;
                    gimbalClearReadMark();
                }
            }
            else
            {
                lastManualTime=HAL_GetTick();   //更新auto模式下手动的时间
                // 直接复制手动控制的代码
                gimbalYawCtrl.wantedAbsoluteAngle += GIMBAL_TASK_CTRL_TIME*(yaw_channel * YAW_RC_SEN + rc_p->mouse.x * YAW_MOUSE_SEN);
                gimbalPitchCtrl.wantedAbsoluteAngle += GIMBAL_TASK_CTRL_TIME*(pitch_channel * PITCH_RC_SEN + rc_p->mouse.y * PITCH_MOUSE_SEN);
            }
        }
    }

    gimbalYawCtrl.wantedAbsoluteAngle = kalmanFilter(&KFP_yaw_wantedagl,gimbalYawCtrl.wantedAbsoluteAngle);
    gimbalPitchCtrl.wantedAbsoluteAngle = kalmanFilter(&KFP_pitch_wantedagl,gimbalPitchCtrl.wantedAbsoluteAngle);
}

// 限制幅度在
void limitAngles()
{
    // 超过最大2PI角度时,限制幅度
    struct gimbalMotorCtrl_s * c;
    int i;
    for(i = 0 ; i < 2; i++)
    {
        c = gimbalCtrl[i];
        if(radFormat(c->wantedAbsoluteAngle - c ->nowAbsoluteAngle) > (PI/2))
            c->wantedAbsoluteAngle = c ->nowAbsoluteAngle + (PI/2);
        if(radFormat(c->wantedAbsoluteAngle - c ->nowAbsoluteAngle) < (-PI/2))
            c->wantedAbsoluteAngle = c ->nowAbsoluteAngle - (PI/2);
    }

    // 限制pitch轴绝对角度。考虑到当前机器人在平地上运动，则全用rad控制。
    if(radFormat(gimbalPitchCtrl.wantedAbsoluteAngle)>PITCH_TEST_RAD_MAX)
        gimbalPitchCtrl.wantedAbsoluteAngle=PITCH_TEST_RAD_MAX;
    if(radFormat(gimbalPitchCtrl.wantedAbsoluteAngle)<PITCH_TEST_RAD_MIN)
        gimbalPitchCtrl.wantedAbsoluteAngle=PITCH_TEST_RAD_MIN;

    // gimbalYawCtrl.wantedAbsoluteAngle = radFormat(gimbalYawCtrl.wantedAbsoluteAngle);
    // gimbalPitchCtrl.wantedAbsoluteAngle = radFormat(gimbalPitchCtrl.wantedAbsoluteAngle);
    
}

void calcPID(void)
{
    uint8_t i;
    struct gimbalMotorCtrl_s * c;
    for(i=0;i<2;i++)
    {
        c=gimbalCtrl[i];
        gimbal_PID_calc(&(c->agl_pid),c->nowAbsoluteAngle,c->wantedAbsoluteAngle);  // 关乎旋转方向的PID控制器
        // 输出了所需旋转速度。

        //对速度进行一阶滤波
        // first_order_filter_cali(&(c->spd_filter),(c->agl_pid).out);

        PID_calc(&(c->spd_pid),c->radSpeed,(c->agl_pid).out);   // 普通的速度控制环
        c->giveVolt=c->spd_pid.out;     //给电压

        if(c->zeroVoltMark==1)
        {
            c->giveVolt=0;
        }
    }

}

void voltchange(void)
{
    #if defined OLD_INFANTRY
    gimbalPitchCtrl.giveVolt=-(gimbalPitchCtrl.giveVolt);
    #elif defined NEW_INFANTRY
    gimbalPitchCtrl.giveVolt=gimbalPitchCtrl.giveVolt;
    #elif defined SENTRY
    gimbalPitchCtrl.giveVolt=gimbalPitchCtrl.giveVolt;
    #endif
}

int16_t * getTriggerCurrentP(void); //从另一个文件获取拨弹轮电机的电流

void gimbal_task(void const *pvParameters)
{
    initGimbalCtrls();
    osDelay(GIMBAL_TASK_INIT_TIME);
    robotMode=getRobotPresentMode();
    rc_p=get_remote_control_point();    //获取遥控器数据和NUC数据指针
    nuc_p=get_nuc_control_point();

    int16_t *triggerCurrentP=getTriggerCurrentP();//获取shoot_task源文件里计算的M2006的电流。
    while(1)
    {
#ifdef FINDING_PID
        if(rc_p->rc.s[1]==3) // 左拨杆在中央
        {
            yawSpdPIDco[0] = yawSpdKp;
            yawSpdPIDco[1] = yawSpdKi;
            yawSpdPIDco[2] = yawSpdKd;

            yawAglPIDco[0] = yawAglKp;
            yawAglPIDco[1] = yawAglKi;
            yawAglPIDco[2] = yawAglKd;

            pitchSpdPIDco[0] = pitchSpdKp;
            pitchSpdPIDco[1] = pitchSpdKi;
            pitchSpdPIDco[2] = pitchSpdKd;

            pitchAglPIDco[0] = pitchAglKp;
            pitchAglPIDco[1] = pitchAglKi;
            pitchAglPIDco[2] = pitchAglKd;            
            
            PID_init(&(gimbalPitchCtrl.spd_pid),PID_POSITION,pitchSpdPIDco,PITCH_VOLT_MAX_OUT,PITCH_VOLT_MAX_IOUT);
            PID_init(&(gimbalYawCtrl.spd_pid),PID_POSITION,yawSpdPIDco,YAW_VOLT_MAX_OUT,YAW_VOLT_MAX_IOUT);
            
            PID_init(&(gimbalPitchCtrl.agl_pid),PID_POSITION,pitchAglPIDco,PITCH_AGL_SPD_MAX_OUT,PITCH_AGL_SPD_MAX_IOUT);
            PID_init(&(gimbalYawCtrl.agl_pid),PID_POSITION,yawAglPIDco,YAW_AGL_SPD_MAX_OUT,YAW_AGL_SPD_MAX_IOUT);

            PID_clear(&gimbalYawCtrl.agl_pid);
            PID_clear(&gimbalYawCtrl.spd_pid);
            PID_clear(&gimbalPitchCtrl.agl_pid);
            PID_clear(&gimbalPitchCtrl.spd_pid);
        }
        else
        {
#endif
            refreshAngleStates();   // 从传感器获得当前角度数据 //test done
            getControlAngles();                  // 从遥控器获得目标角度     //test half done
            limitAngles();
            calcPID();              // 总是控制角度，双环，但测试时需要
            voltchange();
            #ifdef ZERO_CURRENT_SAFE
            CAN_cmd_gimbal(0,0,0,0);
            #else
            CAN_cmd_gimbal(gimbalYawCtrl.giveVolt,gimbalPitchCtrl.giveVolt,*triggerCurrentP,0);       // 暂时只测试云台，不测试发射 //TO DO，加上拨弹轮电流控制函数
            #endif
#ifdef FINDING_PID
        }
#endif
        osDelay(GIMBAL_TASK_CTRL_TIME);
    }
}

fp32 getGimbalYaw(void)
{
    return gimbalYawCtrl.nowAbsoluteAngle;
}

void OLED_gimbal(void)
{
    // 注意，写得太长可能显示不出来
    // OLED_printf(0,0,"agl:%.2f, spd:%.2f", gimbalPitchCtrl.nowAbsoluteAngle, gimbalPitchCtrl.radSpeed);
    // OLED_printf(0,0,"agl:%.2f, spd:%.2f", gimbalYawCtrl.nowAbsoluteAngle, gimbalYawCtrl.radSpeed);
    // OLED_printf(0,0,"acc:%.4f | %.4f | %.4f", get_accel_data_point()[0], 
    //     get_accel_data_point()[1], get_accel_data_point()[2] );
    // OLED_printf(0,0,"P:AGL:%.4f,SPD:%.4f,k_kg:%.4f",
        // gimbalPitchCtrl.nowAbsoluteAngle,gimbalPitchCtrl.radSpeed,KFP_height.Kg);


}


